

import java.util.ArrayList;
import java.lang.Math;

/**
 * @author Thierry Bertin-Mahieux and Jin Yoshikawa
 * @data 02/05/2009
 * Common actions we want the robot to do (for HW1)
 */
public class StudentActions {

    OpenComPort iRobotBAM;
    private double positions[];
    GPS Display; // should not be used any more, remains from HW2
    LUTable straight_cart2robot;	// lookup tables for going in straight line
    LUTable turn_cart2robot;		// lookup tables for turning in place
    
    /**
     * Constructor
     * @param ocp
     * @param Display
     */
    StudentActions(OpenComPort ocp, GPS Display){
    	iRobotBAM = ocp;
    	positions = new double[3];
    	
    	positions[0] = 0.;	// x
    	positions[1] = 0.;	// y
    	positions[2] = 0.;	// angle
    	
    	straight_cart2robot = new LUTable();
    	straight_cart2robot.load_file("LUT_gostraight_cartesian_robot.txt");
    	turn_cart2robot = new LUTable();
    	turn_cart2robot.load_file("LUT_turnangle_cartesian_robot.txt");

    	//this.Display = Display;
    	PrintCurrentPos();
    }

    /**
     * Prints the position vector.
     */
    protected void PrintCurrentPos() {
    	System.out.print("Current pos = {");
    	System.out.print(positions[0] + ",");
    	System.out.print(positions[1] + ",");
    	System.out.println(positions[2] + "}");
    }

    /**
     * Returns the current positions
     * @return The current positions of robot
     */
    public double[]getPositions(){
    	return positions;
    }
    
    /**
     * Stops the robot. PUBLIC USE ONLY! 
     * Updates the current position assuming that the call before
     * was either GoVelocity or TurnVelocity
     * DO NOT USE FOR FUNCTIONS WITHIN STUDENTACTIONS CLASS
     */
    public void StopPublic(){
    	byte[] data = new byte[5];
    	data[0] = (byte)145; //Direct Drive command  
    	data[1] = 0; //[Right velocity high byte]
    	data[2] = 0; //[Right velocity low byte]  
    	data[3] = 0; //[Left velocity high byte]  
    	data[4] = 0; //[Left velocity low byte] 
    	iRobotBAM.Write(data);
    	double distanceangle[] = GetDistanceAngle();
    	positions[2] += distanceangle[1];
    	positions[0] += distanceangle[0]*Math.cos(positions[2]*Math.PI/180);
    	positions[1] += distanceangle[0]*Math.sin(positions[2]*Math.PI/180);
    	PrintCurrentPos();
    }
    
    /**
     * Stops the robot. PRIVATE USE ONLY!
     * This does not update position.
     */
    private void Stop(){
    	byte[] data = new byte[5];
    	data[0] = (byte)145; //Direct Drive command  
    	data[1] = 0; //[Right velocity high byte]
    	data[2] = 0; //[Right velocity low byte]  
    	data[3] = 0; //[Left velocity high byte]  
    	data[4] = 0; //[Left velocity low byte] 
    	iRobotBAM.Write(data);
    	PrintCurrentPos();
    }
    
    public void GoDistance(int dist) {
    	int distInRobotSpace;
    	if (straight_cart2robot.size() == 0)
    		distInRobotSpace = dist;
    	else // use lookup table
    		distInRobotSpace = (int) straight_cart2robot.lookup(Math.abs(dist));
    	if (dist < 0)
    		distInRobotSpace = -distInRobotSpace;
    	
    	byte[] data = new byte [5];
    	data[0] = (byte)145;  //Direct Drive command
    	if (distInRobotSpace > 0){
    		data[1] = (byte)(200 >> 8 & 0x00FF);    //[Right velocity high byte]
    		data[2] = (byte)(200 & 0x00FF);
    		data[3] = (byte)(200 >> 8 & 0x00FF);    //[Left velocity high byte]
    		data[4] = (byte)(200 & 0x00FF);
    	}
    	else{
    		data[1] = (byte)(-200 >> 8 & 0x00FF);    //[Right velocity high byte]
    		data[2] = (byte)(-200 & 0x00FF);
    		data[3] = (byte)(-200 >> 8 & 0x00FF);    //[Left velocity high byte]
    		data[4] = (byte)(-200 & 0x00FF);
    	}
      iRobotBAM.Write(data);
      // wait distance
      byte data2[] = new byte[3];
      data2[0] = (byte)156; // Wait distance
      data2[1] = (byte)(distInRobotSpace >> 8 & 0x00FF); 
      data2[2] = (byte)(distInRobotSpace & 0x00FF);
      iRobotBAM.Write(data2);
      // stop 
      Stop();
        
      positions[0]+=Math.cos(positions[2]*Math.PI/180)*dist;
      positions[1]+=Math.sin(positions[2]*Math.PI/180)*dist;
      PrintCurrentPos();
    };
    
    /**
     * Go with specified velocity
     * @param velo Negative means backwards.
     */
    public void GoVelocity(int velo) {
    	GetDistance();	// Clear robot's internal distance count
    	GetAngle();		// Clear robot's internal angle count
    	byte[] data = new byte [5];
    	// start moving at velo mm/s, velo <= 500
    	data[0] = (byte)145;  //Direct Drive command
      data[1] = (byte)(velo>>8 & 0x00FF);    //[Right velocity high byte] 
      data[2] = (byte)(velo&0x00FF);  //[Right velocity low byte]
      data[3] = (byte)(velo>>8 & 0x00FF);    //[Left velocity high byte]
      data[4] = (byte)(velo & 0x00FF);  //[Left velocity low byte]
      iRobotBAM.Write(data);
    }
    
    /**
     * Turn for a specified angle
     * @param a Angle to turn. Positive means counterclockwise.
     */
    public void TurnAngle(int a) {
    	int angleInRobotSpace;
    	if (turn_cart2robot.size() == 0)
    		angleInRobotSpace = a;
    	else // use lookup table
    		angleInRobotSpace = (int)turn_cart2robot.lookup(Math.abs(a));
    	if (a < 0)
    		angleInRobotSpace = -angleInRobotSpace;
    	byte[] data = new byte [5];
    	data[0] = (byte)145; 
    	if (angleInRobotSpace > 0){ 
    		data[1] = 0; 
    		data[2] = (byte)200; 
    		data[3] = (byte)((-200) >> 8 & 0x00FF); 
    		data[4] = (byte)((-200) & 0x00FF); 
    	} 
    	else { 
    		data[1] = (byte)((-200) >> 8 & 0x00FF); 
    		data[2] = (byte)((-200) & 0x00FF); 
    		data[3] = 0; 
    		data[4] = (byte)200; 
    	} 
    	iRobotBAM.Write(data); 
    	// wait angle
    	byte data2[] = new byte[3]; 
    	data2[0] = (byte)157; 
    	data2[1] = (byte)(angleInRobotSpace >> 8 & 0x00FF); 
    	data2[2] = (byte)(angleInRobotSpace & 0x00FF); 
    	iRobotBAM.Write(data2); 
    	Stop();
    	
    	positions[2]=(positions[2] + a + 360) % 360;
    	PrintCurrentPos();
    };
    
    /**
     * Turn while specifying a velocity
     * @param velo Positive velocity means leftward turn.
     */
    public void TurnVelocity(int velo){
    	GetDistance();	// Clear robot's internal distance count
    	GetAngle();		// Clear robot's internal angle count
    	byte[] data = new byte [5];
    	// start moving at velo mm/s, velo <= 500
    	data[0] = (byte)145;  //Direct Drive command
      data[1] = (byte)(velo>>8 & 0x00FF);    //[Right velocity high byte] 
      data[2] = (byte)(velo&0x00FF);  //[Right velocity low byte]
      data[3] = (byte)(-velo>>8 & 0x00FF);    //[Left velocity high byte]
      data[4] = (byte)(-velo & 0x00FF);  //[Left velocity low byte]
      iRobotBAM.Write(data);
    }

    /**
     * Turn while specifying the distance to travel and
     * the radius of the circle along which you circumnavigate.
     * In other words, travel along the circumference of
     * a circle with radius 'radius' for a distance 'distance.'
     * @param distance The arclength of the circle to circumnavigate
     * @param radius The radius of the circle to circumnavigate.
     * Positive radius is a leftward circle.
     */
    public void Turn(int distance, int radius){
    	//GetDistance();	// sets robot's internal distance count to zero
    	//GetAngle();		// sets robot's internal angle count to zero
    	GetDistanceAngle(); // sets robot's internal angle count to zero
    	byte[] turn = new byte[5];
    	turn[0] = (byte)137;
    	turn[1] = (byte)(200 >> 8 & 0x00FF);
    	turn[2] = (byte)(200 & 0x00FF);
    	turn[3] = (byte)(radius >> 8 & 0x00FF);
    	turn[4] = (byte)(radius & 0x00FF);
    	iRobotBAM.Write(turn);
    	byte wait[] = new byte[3];
    	wait[0] = (byte)156;
    	wait[1] = (byte)(distance >> 8 & 0x00FF);
    	wait[2] = (byte)(distance & 0x00FF); 
    	iRobotBAM.Write(wait);
    	Stop();
    	
    	// Calculations here to find new xpos,ypos,angle
    	double dist_angle[] = GetDistanceAngle();
		double dist = dist_angle[0];	
		double angle = dist_angle[1];
		Double inner_angle = ( dist / Math.abs( radius ) ) * 180. / Math.PI;
		Double straight_dist;
		Double start_angle;
		if (inner_angle <= 180) {
			start_angle = (180. - inner_angle) / 2.;
			straight_dist = Math.abs(radius) / Math.sin(start_angle * Math.PI / 180.) * Math.sin(inner_angle * Math.PI / 180.);
		}
		else { // inner angle > 180
			inner_angle = 360. - inner_angle;
			straight_dist = 2. * Math.abs(radius) * Math.sin(inner_angle * 2. * Math.PI / 360.);
			start_angle = - (180. - inner_angle) / 2.;	
		}
		if (straight_dist.isNaN() || start_angle.isNaN()) {
			System.out.println("NaN in Turn");
			positions[2] = (positions[2] + angle + 360.) % 360;
		}
		else {
    		double orientation;
    		if (radius > 0)
    			orientation = (positions[2] + 90 - start_angle + 360.) % 360;
    		else 
    			orientation = (positions[2] - 90 + start_angle + 360.) % 360;
    		orientation *= 2 * Math.PI / 360.; // in radians
    		positions[0] += Math.cos(orientation) * dist;
    		positions[1] += Math.sin(orientation) * dist;
    		positions[2] = (positions[2] + angle + 360.) % 360;
		}
		PrintCurrentPos();
    }
    
    /**
     * Make a square
     */
    public void MakeSquare() {
	// simply call previous functions
    	for (int k = 0; k < 4; k++) {
    		GoDistance(400);
    		TurnAngle(90);
    	}
    };

    /**
     * Turn until the angle specified is reached
     * @param angle Angle to be reached
     */
    protected void TurnTillAngle(int angle) {
    	int presentangle = (int)positions[2]%360;
	if (presentangle>180)
		TurnAngle((int)(angle+360-presentangle));
	else
		TurnAngle((int)(angle-presentangle));
    	PrintCurrentPos();
    }
    
    /**
     * FollowWall will run a specified number of iterations of FollowWallHelper
     * @param iter Number of iterations of FollowWallHelper to run.
     */
    public void FollowWall(int iter) {
    	String mode = "start";
    	for (int i = 0; i < iter; i++)
    		mode = FollowWallHelper(mode);
    }
    
    /**
     * One step of Wall Following
     * @param mode mode to do step in
     * @return Mode that the wall follow is the step has been executed
     */
    public String FollowWallHelper(String mode) {
    	// set up bump data
    	double wall_bumps[] = GetWallBumps();
		int wall = (int) wall_bumps[0]; //readSensor("wall");
		int bump = (int) wall_bumps[1] + (int)wall_bumps[2];		
    	// go!
	if (bump>0){ 					// Robot is bumping into wall
		GoDistance(-20);				//	Move back 1cm
		TurnAngle(30);				//	Turn 45 degrees clockwise in place
		mode = "followwall";
	}
    	else{							// Robot is NOT bumping into wall
		if (mode.equals("followwall")){	//	Mode is "followwall"
			if (wall<5)				//		Case 0:	Sudden wall dropoff
				mode = "bumpwall";	//				5, not 0, because of noise
			else if (wall<50)			// 		Case 1:	Left curving wall
				Turn(20, 800);		//				Turn a little left
    			else if (wall<250)		// 		Case 2:	Straight wall
    				GoDistance(20);		//				Go straight
    			else					// 		Case 3:	Right curving wall
    				Turn(20, -800);		//				Turn a little right
		}
		else if(mode.equals("bumpwall"))	// 	Mode is "bumpwall"
			Turn(20,-200); 			// 		Bump
		else if (mode.equals("start"))	// 	Mode is "start"
			GoDistance(20);
		else
			System.err.println("bad mode name: " + mode);
	}
	PrintCurrentPos();
	return mode;
    }
  
    /**
     * Read more than one sensor.
     * @param SensorsOn
     * @return A string that gives the sensor statuses for all sensors specified.
     */
    public String ReadSensors(ArrayList<String> SensorsOn) {
    	String result = "";
    	Integer sensor_res;
    	for (String name : SensorsOn){
    		sensor_res = readSensor(name);
    		result += "<" + name + "=" + sensor_res.toString() + ">";
    	}
    	return result;
    };

    /**
     * Get the distance in mm since we asked for it
     * @return distance
     */
    public int GetDistance() {
    	byte[] data = new byte[2];
    	data[0]= (byte)142;
    	data[1]= (byte) 19;
    	// send request
    	iRobotBAM.Write(data);
    	// get answer
    	byte[] sensor = new byte[2];
	iRobotBAM.Read(sensor);
	int res = (int)(sensor[0] << 8) + (int)(sensor[1]&0x00FF);
	return res;
    }

    /**
     * Get the angle in degrees since we asked for it
     * @return angle
     */
    public int GetAngle() {
    	byte[] data = new byte[2];
    	data[0]= (byte)142;
    	data[1]= (byte) 20;
    	// send request
    	iRobotBAM.Write(data);
    	// get answer
    	byte[] sensor = new byte[2];
	iRobotBAM.Read(sensor);
	int res = (int)(sensor[0] << 8) + (int)(sensor[1]&0x00FF);
	return res;
    }
    
    /**
     * Get Distance and Angle in one call to the robot by calling packet 2
     * @return [distance,angle]
     */
    protected double[] GetDistanceAngle() {
    	double[] res = {0,0};
    	// call packet 2
    	byte[] data = new byte[2];
    	data[0]= (byte)142;
    	data[1]= (byte) 2;
    	// send request
    	iRobotBAM.Write(data);
    	// get answer
    	byte[] sensor = new byte[6];
	iRobotBAM.Read(sensor);
	// distance then angle
	byte temp[] = {sensor[2],sensor[3]};
	res[0] = (int)(sensor[2] << 8) + (int)(sensor[3]&0x00FF);
	temp[0] = sensor[4]; temp[1] = sensor[5];
	res[1] = (int)(sensor[4] << 8) + (int)(sensor[5]&0x00FF);
    	return res;
    }
    
    /**
     * Get wall, bump left and bump right in one call to the robot by calling packet 6
     * Maybe over expensive, packet 6 is huge!
     * @return [wall,bumpLeft,bumpRight]
     */
    protected double[] GetWallBumps() {
    	double[] res = {0,0,0};
    	// call packet 1
    	byte[] data = new byte[2];
    	data[0]= (byte)142;
    	data[1]= (byte) 1;
    	// send request
    	iRobotBAM.Write(data);
    	// get answer
    	byte[] sensor = new byte[52];
	iRobotBAM.Read(sensor);
	// wall than bumpLeft then bumpRight
	res[0] = (int)((sensor[1]&0x00FF) << 8) + (int)(sensor[1]&0x00FF);
	res[1] = (sensor[0] & (1 << 1)) >> 1;
	res[2] = (sensor[0] & 1);
	return res;
    }
  
    /** 
     * read one sensor specified by its name (same as buttons)
     */
    protected int readSensor(String name){
    	byte[] data = new byte[2];
    	int res = 0;
    	data[0]= (byte)142;
    	if (name.equals("wheeldropCaster")) data[1]=(byte)7;
    	else if (name == "wheeldropLeft") data[1]=(byte)7;
    	else if (name == "wheeldropRight") data[1]=(byte)7;
    	else if (name == "bumpLeft") data[1]=(byte)7;
    	else if (name == "bumpRight") data[1]=(byte)7;
    	else if (name == "wall") data[1]=(byte)27;
    	else if (name == "cliffLeft") data[1]=(byte)9;
    	else if (name == "cliffFrontLeft") data[1]=(byte)10;
    	else if (name == "cliffFrontRight") data[1]=(byte)11;
    	else if (name == "cliffRight") data[1]=(byte)12;
    	else if (name == "virtualWall") data[1]=(byte)13;
    	else {
    		System.err.println("bad sensor name: " + name);
    		return -1;
    	}
    	iRobotBAM.Write(data);
    	if (name.equals("wall")){
    		byte[] sensor = new byte[2];
    		iRobotBAM.Read(sensor);
    		res = (int)((sensor[0]&0x00FF)<<8)+ (int)(sensor[1]&0x00FF);
    	}
    	else{
    		byte[] sensor = new byte[1];
    		iRobotBAM.Read(sensor);
    		res = sensor[0]; 
    		if (name.equals("wheeldropCaster"))
    			res = (res & (1 << 4)) >> 4;
    		else if (name.equals("wheeldropLeft"))
    			res = (res & (1 << 3)) >> 3;
    		else if (name.equals("wheeldropRight"))
    			res = (res & (1 << 2)) >> 2;
    		else if (name.equals("bumpLeft"))
    			res = (res & (1 << 1)) >> 1;
    		else if (name.equals("bumpRight"))
    			res = (res & 1);
    		else{}
    	}
    	return res;
    }

    /**
     * Main function of HW3
     * @param goalpositions Target point
     */
    public void Bug2(double goalpositions[]){
    	
    	double[] startpositions = new double[3];
    	startpositions[0] = positions[0];
    	startpositions[1] = positions[1];

	int mlineangle = (int)(Math.atan2(	goalpositions[1]-startpositions[1],
							goalpositions[0]-startpositions[0])
							* 180/Math.PI);
    	
    	// states and modes
    	String state = "headtowardsgoal";
    	String mode = ""; // for followwall
    	double wall_bumps[] = GetWallBumps();
	int bump = (int) wall_bumps[1] + (int)wall_bumps[2];
    	
    	// position where we hit the line
    	double hitpositions[] = new double[3];
    	
    	System.out.println("Positions=" + positions[0] + "," +positions[1]);
    	System.out.println("Start pos=" + startpositions[0] + "," + startpositions[1]);
    	System.out.println("Goal pos=" + goalpositions[0] + "," + goalpositions[1]);
    	
    	// Turn towards goal
    	TurnTillAngle(mlineangle);
				
    	// While we have not reached the goal, loop
    	while ( !goal_reached(positions, goalpositions) ){
			
    		System.out.println("state=" + state);
			
		if (state.equals("headtowardsgoal")){
			System.out.println("BUG2: Heading toward goal");
    			GoDistance(35);
    			wall_bumps = GetWallBumps();
    			bump = (int) wall_bumps[1] + (int)wall_bumps[2];
    			if (bump>0){
    				hitpositions[0] = positions[0];	// store x where we hit the object
    				hitpositions[1] = positions[1];	// store y where we hit the object
    				state = "followwall";
    				while (distance(hitpositions, positions)<40){
    				mode = FollowWallHelper("bumpwall");
    				}
    			}else{}
    		}
    		else if (state.equals("followwall")){     		// follow wall mode
    			// If, on m-line
    			if (On_Line(startpositions, goalpositions, positions) && 
    					isBetween(positions, startpositions, goalpositions)){
    				
    				System.out.println("BUG2: Following wall:  On m-line");
    				
    				// if closer to goal than hitpoint, turn towards the goal
    				if (distance(positions, goalpositions) <
    						distance(hitpositions, goalpositions) + 20) {
    					System.out.println(
    							"BUG2: Following wall: Closer to goal than before");
    					TurnTillAngle(mlineangle);
    					state = "headtowardsgoal";
    					mode = "bumpwall";
    				}
    				
    				// same place
    				else if ( Math.abs(distance(positions, goalpositions) -
    						distance(hitpositions, goalpositions)) < 20){
    					System.out.println("BUG2: Following wall:  No path to goal");
    					return;
    				}
    				
    				// farther from goal
    				else {
    					System.out.println(
    							"BUG2: Following wall:  Further to goal than before");
    					mode = FollowWallHelper(mode);
    				}
    			}
    			// Else, not on m-line
    			else{
    				System.out.println("BUG2: Following wall: Not on m-line");
    				mode = FollowWallHelper(mode);
    			}
    		}
    		else
    			System.err.println("bad mode name: " + state);
    	}
    	System.out.println("Goal reached");
    }
    
    /**
     * Returns whether we are on a line or not.
     * @param start point1 of line
     * @param goal point2 of line
     * @param pos Current position
     * @return True if on the M-line+-20mm, False otherwise
     */
    private boolean On_Line(double[] start, double[] goal, double[] pos){
    	Double m = (goal[1] - start[1])/(goal[0]-start[0]);    	
    	if (m.isInfinite())
    		return Math.abs(pos[0] - start[0]) < 20;
    	else
    		return Math.abs(pos[1] - m*(pos[0]-start[0])+start[1]) < 20;
    }
    
    /**
     * Checks whether we reached the goal
     * @param positions Our position
     * @param goalpositions Goal position
     * @return true if we're at the goal, false otherwise
     */
    private boolean goal_reached(double[] positions, double[] goalpositions) {
    	double dist = distance(positions, goalpositions);
    	return dist < 20;
    }
     
    /**
     * Checks whether position a is between position b and position c
     */
    private boolean isBetween(double a[], double b[], double c[]){
    	boolean xisbetween;
    	boolean yisbetween;
    	if (Math.abs(b[0]-c[0]) < 10)
    		xisbetween =	(a[0] < b[0] && a[0] > c[0]) ||
    					(a[0] > b[0] && a[0] < c[0]) ||
    					(Math.abs(a[0]-b[0]) < 10) ||
    					(Math.abs(a[0]-c[0]) < 10);
    	else
    		xisbetween =	(a[0] < b[0] && a[0] > c[0]) ||
					(a[0] > b[0] && a[0] < c[0]);
    	if (Math.abs(b[1]-c[1]) < 10)
    		yisbetween =	(a[1] < b[1] && a[1] > c[1]) ||
					(a[1] > b[1] && a[1] < c[1]) ||
					(Math.abs(a[1]-b[1]) < 10) ||
					(Math.abs(a[1]-c[1]) < 10);
    	else
    		yisbetween =	(a[1] < b[1] && a[1] > c[1]) ||
					(a[1] > b[1] && a[1] < c[1]);
    	return  xisbetween && yisbetween;
    }
    
    /**
     * Euclidean distance between two points
     * @param position1 Coordinates of the first point
     * @param position2 Coordinates of the second point
     * @return
     */
    private double distance(double position1[], double position2[]){
    	double distance = Math.sqrt(Math.pow(position1[0]-position2[0],2)+Math.pow(position1[1]-position2[1],2));
    	return distance;
    }
}
